#include "ricci.h"
#include <iostream>


ricci::ricci(Mesh & mesh_, double stepSize_, double Thres_)
	: mesh(&mesh_), stepSize(stepSize_), Thres(Thres_)
{
	int character = mesh->n_vertices() - mesh->n_edges() + mesh->n_faces();
	int genus = 1 - character / 2;
	int nV = mesh->n_vertices();

	bool hasboundary = false;

	for (auto eh : mesh->edges())
	{
		if (mesh->is_boundary(eh))
		{
			hasboundary = true;
			break;
		}
	}

	if (hasboundary)
	{
		genusnum = Genus2::withboundary;
		std::cout << "Error: not completed yet\n";
		return;
	}
	else if (genus == 0)
	{
		genusnum = Genus2::genus0;
		goalK = Eigen::VectorXd::Constant(nV, (2 * character * M_PI) / nV);
		aveK = (2 * character * M_PI) / nV;
	}
	else if (genus == 1)
	{
		genusnum = Genus2::genus1;
		//std::cout << "Error: not completed yet\n";
		//return;
		goalK = Eigen::VectorXd::Constant(nV, 0.0);
		aveK = 0.0;
	}
	else
	{
		genusnum = Genus2::highgenus;
		std::cout << "Error: not completed yet\n";
		return;
	}









}

void ricci::init()
{
	auto nV = mesh->n_vertices();
	auto nE = mesh->n_edges();

	currentradius.reserve(nV);
	inversedist.reserve(nE);
	currrentlength.reserve(nE);
	orilength.reserve(nE);

	for (auto vh : mesh->vertices())
	{
		double radii = INFINITY;

		for (auto evh : mesh->ve_range(vh))
		{
			double len = mesh->calc_edge_length(evh);
			if (len < radii)
				radii = len;
		}

		radii /= 3;

		currentradius.push_back(radii);

	}


	for (auto eh : mesh->edges())
	{
		auto heh = mesh->halfedge_handle(eh, 0);

		double len = mesh->calc_edge_length(eh);

		double r1 = currentradius[mesh->to_vertex_handle(heh).idx()];
		double r2 = currentradius[mesh->from_vertex_handle(heh).idx()];

		double invd = (len * len - std::pow(r1, 2) - std::pow(r2, 2)) / (2 * r1 * r2);

		inversedist.push_back(invd);
		currrentlength.push_back(len);
		orilength.push_back(len);
	}
	
	currentStatus();
	
}

void ricci::calculateradii()
{
	double error = (goalK - currentK).norm();
	std::cout << error << '\n';

	while (error > Thres)
	{
		for (auto vh : mesh->vertices())
		{
			currentradius[vh.idx()] = exp((goalK[vh.idx()] - currentK[vh.idx()])*stepSize) * currentradius[vh.idx()];
		}

		//delauneyFlip();
		calculatelength();
		Flip();
		currentStatus();

		error = (goalK - currentK).norm();
		std::cout << error << '\n';
	}

}

void ricci::calculatePos()
{
	calculateballradii();

	auto nF = mesh->n_faces();
	auto nV = mesh->n_vertices();
	std::vector<int> isFaceFixed(nF, 0);
	std::list<OpenMesh::HalfedgeHandle> worklist;

	auto face = mesh->face_handle(0);
	OpenMesh::Vec3d midPoint(0., 0., 0.);
	for (auto v : mesh->vertices())
		midPoint += mesh->point(v);
	midPoint /= nV;
	double minDist = INFINITY;
	for (auto f : mesh->faces())
	{
		double dist = 0.;
		for (auto v : mesh->fv_range(f))
		{
			dist += (mesh->point(v) - midPoint).norm();
		}
		if (dist < minDist)
		{
			minDist = dist;
			face = f;
		}
	}
	isFaceFixed[face.idx()] = 1;

	auto he0 = mesh->halfedge_handle(face);
	auto he1 = mesh->next_halfedge_handle(he0);
	auto he2 = mesh->next_halfedge_handle(he1);

	auto l0 = currrentlength[mesh->edge_handle(he0).idx()];
	auto l1 = currrentlength[mesh->edge_handle(he1).idx()];
	auto l2 = currrentlength[mesh->edge_handle(he2).idx()];

	double cosr = (ballradii * ballradii * 2 - l0 * l0) / (2 * ballradii * ballradii);
	double sinr = std::sqrt(1 - cosr * cosr);

	mesh->set_point(mesh->from_vertex_handle(he0), OpenMesh::Vec3d(ballradii, 0, 0));
	mesh->set_point(mesh->from_vertex_handle(he1), OpenMesh::Vec3d(ballradii * cosr, ballradii * sinr, 0));
	mesh->set_point(mesh->from_vertex_handle(he2), calculatePosOnBall(OpenMesh::Vec3d(ballradii, 0, 0), OpenMesh::Vec3d(ballradii * cosr, ballradii * sinr, 0), l2, l1));

	worklist.push_back(mesh->opposite_halfedge_handle(he0));
	worklist.push_back(mesh->opposite_halfedge_handle(he1));
	worklist.push_back(mesh->opposite_halfedge_handle(he2));

	while (!worklist.empty())
	{
		auto he = worklist.front();
		worklist.pop_front();
		if (isFaceFixed[mesh->face_handle(he).idx()])
		{
			continue;
		}
		auto v = mesh->to_vertex_handle(mesh->next_halfedge_handle(he));
		bool isVFixed = false;
		for (auto vf : mesh->vf_range(v))
		{
			if (isFaceFixed[vf.idx()])
			{
				isVFixed = true;
				break;
			}
		}
		if (isVFixed)
		{
			isFaceFixed[mesh->face_handle(he).idx()] = true;
			continue;
		}

		// the vertex is not fixed, fix it now
		if (genusnum == Genus2::genus0)
		{
			auto v0 = mesh->from_vertex_handle(he), v1 = mesh->to_vertex_handle(he);
			auto l1 = currrentlength[mesh->edge_handle(mesh->next_halfedge_handle(he)).idx()];
			auto l0 = currrentlength[mesh->edge_handle(mesh->next_halfedge_handle(mesh->next_halfedge_handle(he))).idx()];

			auto pos = calculatePosOnBall(mesh->point(v0), mesh->point(v1), l0, l1);
			mesh->set_point(v, pos);

		}

		isFaceFixed[mesh->face_handle(he).idx()] = true;

		auto opp1 = mesh->opposite_halfedge_handle(mesh->next_halfedge_handle(he));
		auto opp2 = mesh->opposite_halfedge_handle(mesh->next_halfedge_handle(mesh->next_halfedge_handle(he)));
		if (!isFaceFixed[mesh->face_handle(opp1).idx()])
			worklist.push_back(opp1);
		if (!isFaceFixed[mesh->face_handle(opp2).idx()])
			worklist.push_back(opp2);
	}



}

void ricci::currentStatus()
{
	auto nV = mesh->n_vertices();
	Eigen::VectorXd theta = Eigen::VectorXd::Constant(nV, 0.);
	Eigen::VectorXd totalK = Eigen::VectorXd::Constant(nV, 2*M_PI);

	for (auto he : mesh->halfedges())
	{
		auto len0 = currrentlength[mesh->edge_handle(he).idx()];
		auto len1 = currrentlength[mesh->edge_handle(mesh->next_halfedge_handle(he)).idx()];
		auto len2 = currrentlength[mesh->edge_handle(mesh->next_halfedge_handle(mesh->next_halfedge_handle(he))).idx()];
		double cos_ = (len1 * len1 + len2 * len2 - len0 * len0) / (2 * len1 * len2);
		auto angle = acos(cos_);
		theta(mesh->to_vertex_handle(mesh->next_halfedge_handle(he)).idx()) += angle;
	}

	currentK = totalK - theta;

}

void ricci::delauneyFlip()
{
	bool isClean = false;

	while (!isClean)
	{
		isClean = true;
		for (auto eh : mesh->edges())
		{
			if (!mesh->is_flip_ok(eh))
				continue;
			auto heh0 = mesh->halfedge_handle(eh, 0);
			auto heh1 = mesh->halfedge_handle(eh, 1);

			auto hed = mesh->next_halfedge_handle(heh0);
			auto hea = mesh->next_halfedge_handle(hed);
			auto heb = mesh->next_halfedge_handle(heh1);
			auto hec = mesh->next_halfedge_handle(heb);

			auto vhs = mesh->to_vertex_handle(heh0);
			auto vhp = mesh->to_vertex_handle(hed);
			auto vhq = mesh->to_vertex_handle(hea);
			auto vhr = mesh->to_vertex_handle(heb);

			auto eha = mesh->edge_handle(hea);
			auto ehb = mesh->edge_handle(heb);
			auto ehc = mesh->edge_handle(hec);
			auto ehd = mesh->edge_handle(hed);

			double a = inversedist[eha.idx()];
			double b = inversedist[ehb.idx()];
			double c = inversedist[ehc.idx()];
			double d = inversedist[ehd.idx()];
			double e = inversedist[eh.idx()];

			double f = trans(a, b, c, d, e);

			double s = currentradius[vhs.idx()];
			double r = currentradius[vhr.idx()];
			double p = currentradius[vhp.idx()];
			double q = currentradius[vhq.idx()];

			double delau1 = std::sqrt(Delta(b, c, e)) / p + std::sqrt(Delta(a, d, e)) / r;
			double delau2 = std::sqrt(Delta(c, d, f)) / p + std::sqrt(Delta(a, b, f)) / r;

			if (delau1 > delau2)
			{
				isClean = false;
				mesh->flip(eh);
				std::cout << 1;
				inversedist[eh.idx()] = f;

			}



		}


	}


}

void ricci::Flip()
{
	bool isClean = false;

	while (!isClean)
	{
		isClean = true;
		for (auto eh : mesh->edges())
		{
			if (!mesh->is_flip_ok(eh))
				continue;
			auto heh0 = mesh->halfedge_handle(eh, 0);
			auto heh1 = mesh->halfedge_handle(eh, 1);

			auto hed = mesh->next_halfedge_handle(heh0);
			auto hea = mesh->next_halfedge_handle(hed);
			auto heb = mesh->next_halfedge_handle(heh1);
			auto hec = mesh->next_halfedge_handle(heb);

			auto vhs = mesh->to_vertex_handle(heh0);
			auto vhp = mesh->to_vertex_handle(hed);
			auto vhq = mesh->to_vertex_handle(hea);
			auto vhr = mesh->to_vertex_handle(heb);

			auto eha = mesh->edge_handle(hea);
			auto ehb = mesh->edge_handle(heb);
			auto ehc = mesh->edge_handle(hec);
			auto ehd = mesh->edge_handle(hed);

			double a = inversedist[eha.idx()];
			double b = inversedist[ehb.idx()];
			double c = inversedist[ehc.idx()];
			double d = inversedist[ehd.idx()];
			double e = inversedist[eh.idx()];

			double f = trans(a, b, c, d, e);

			double s = currentradius[vhs.idx()];
			double r = currentradius[vhr.idx()];
			double p = currentradius[vhp.idx()];
			double q = currentradius[vhq.idx()];


			double la = currrentlength[eha.idx()];
			double lb = currrentlength[ehb.idx()];
			double lc = currrentlength[ehc.idx()];
			double ld = currrentlength[ehd.idx()];
			double le = currrentlength[eh.idx()];

			double cos1 = (la * la + ld * ld - le * le) / (2 * la * ld);
			double cos2 = (lb * lb + lc * lc - le * le) / (2 * lb * lc);

			if (cos1 < -0.95|cos2 < -0.95)
			{
				isClean = false;
				mesh->flip(eh);
				inversedist[eh.idx()] = f;
				currrentlength[eh.idx()] = std::sqrt(r * r + p * p + 2 * r * p * f);
			}



		}

	}







}

void ricci::calculatelength()
{
	for (auto eh : mesh->edges())
	{
		auto heh = mesh->halfedge_handle(eh, 0);
		double r0 = currentradius[mesh->to_vertex_handle(heh).idx()];
		double r1 = currentradius[mesh->from_vertex_handle(heh).idx()];
		double invd = inversedist[eh.idx()];

		double length = std::sqrt(r0 * r0 + r1 * r1 + 2 * invd * r0 * r1);

		currrentlength[eh.idx()] = length;
	}
}

void ricci::calculateballradii()
{

	double totalarea = 0;

	for (auto fh : mesh->faces())
	{
		auto he = mesh->halfedge_handle(fh);
		auto he0 = mesh->next_halfedge_handle(he);
		auto he1 = mesh->next_halfedge_handle(he0);

		double l = currrentlength[mesh->edge_handle(he).idx()];
		double l0 = currrentlength[mesh->edge_handle(he0).idx()];
		double l1 = currrentlength[mesh->edge_handle(he1).idx()];

		double area = std::sqrt((l + l0 + l1) * (l0 + l1 - l) * (l - l0 + l1) * (l + l0 - l1));
		area /= 4;

		totalarea += area;
	}

	ballradii = std::sqrt(totalarea / (4 * M_PI));
}

OpenMesh::Vec3d ricci::calculatePosOnBall(OpenMesh::Vec3d a, OpenMesh::Vec3d b, double la, double lb)
{
	double p1[3] = { a[0], a[1], a[2] };
	double p2[3] = { b[0], b[1], b[2] };
	double ha = (a[0] * a[0] + a[1] * a[1] + a[2] * a[2] - la * la + ballradii * ballradii) / 2;
	double hb = (b[0] * b[0] + b[1] * b[1] + b[2] * b[2] - lb * lb + ballradii * ballradii) / 2;

	p2[1] -= (p1[1] * p2[0] / p1[0]);
	p2[2] -= (p1[2] * p2[0] / p1[0]);
	hb -= (ha * p2[0] / p1[0]);
	p2[0] = 0;

	p1[2] -= (p1[1] * p2[2] / p2[1]);
	ha -= (p1[1] * hb / p2[1]);

	p1[2] /= p1[0];
	ha /= p1[0];
	p2[2] /= p2[1];
	hb /= p2[1];

	double c = 1 + p1[2] * p1[2] + p2[2] * p2[2];
	double d = 2 * (ha * p1[2] + hb * p2[2]);
	double e = ha * ha + hb * hb - ballradii * ballradii;

	double x1 = (d + std::sqrt(d * d - 4 * c * e)) / 2 * c;
	double x2 = (d - std::sqrt(d * d - 4 * c * e)) / 2 * c;

	double l1[3] = { ha - p1[2] * x1 - a[0], hb - p2[2] * x1 - a[1], x1 - a[2] };
	double l2[3] = { b[0] - a[0], b[1] - a[1], b[1] - a[1] };

	double n[3] = { l1[1] * l2[2] - l1[2] * l2[1],l1[0] * l2[2] - l1[2] * l2[0], l1[0] * l2[1] - l1[1] * l2[0] };
	double m = a[0] * n[0] + a[1] * n[1] + a[2] * n[2];


	if (m > 0)
	{
		return OpenMesh::Vec3d(ha - p1[2] * x1, hb - p2[2] * x1, x1);
	}


	return OpenMesh::Vec3d(ha - p1[2] * x2, hb - p2[2] * x2, x2);
}

void ricci::checkdelauney()
{










}

void ricci::callen()
{
}


